#include <motionControl.hpp>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Eigen>
#include <Eigen/Sparse>
#include <Eigen/SVD>
#include <math.h>
#include <qpOASES.hpp>
#define PI 3.1415926
#define TIME_STEP 10

using namespace std;
using namespace Eigen;

void MotionControl::MotionContr(float tP, Robot *robot)
{
    timePeriod = tP;
    targetCoMVelocity << 0.0, 0.0, 0.0;
    L1 = 0.132;
    L2 = 0.138;
    L3 = 0.047;
    width = 0.087;
    length = 0.174;
    shoulderPos << width/2, length/2, width/2, -length/2, -width/2, length/2, -width/2, -length/2;  // X-Y: LF, RF, LH, RH
    legPresentPos_target << 0, 0, -0.24;
    timeForStancePhase << 0, 0.29, 0.30, 0.59, 0.30, 0.59, 0, 0.29;
    for (int i=0; i<12; i++)
    {
      Ps[i] = robot->getPositionSensor(positionName[i]);
      Ps[i]->enable(TIME_STEP);
      Tor[i] = robot->getMotor(motorName[i]);
    }
    for (int i=0; i<4; i++)
    {
      Ts[i] = robot->getTouchSensor(touchsensorName[i]);
      Ts[i]->enable(TIME_STEP);
    }
    for (int i = 0; i < 6; i++)
    {
        Ds[i] = robot->getDistanceSensor(distanceSensorName[i]);
        Ds[i]->enable(TIME_STEP);
    }
    imu = robot->getInertialUnit("inertial unit");
    imu->enable(TIME_STEP);
    gps = robot->getGPS("global_gps");
    gps->enable(TIME_STEP);
}
void MotionControl::setInitPos(Matrix<float, 4, 3> initPosition)
{
    legPresentPos = initPosition;
    legCmdPos = initPosition;
    body_x = initPosition(0, 0);
    body_y = initPosition(0, 1);
}
void MotionControl::Sensor_update()
{
    Vector<float, 12> jointLastPos;
    for (int i = 0; i < 12; i++)
    {
        jointLastPos(i) = jointPresentPos(i);
    }
    Vector<float, 3> Lastimu_num;
    for (int i = 0; i < 3; i++)
    {
        Lastimu_num(i) = imu_num(i);
    }
    
    for (int i=0; i<12; i++)
    {
        jointPresentPos(i) = Ps[i]->getValue();
    }
    float temp_touch1 = Ts[0]->getValue();
    float temp_touch2 = Ts[1]->getValue();
    float temp_touch3 = Ts[2]->getValue();
    float temp_touch4 = Ts[3]->getValue();
    state << temp_touch1, temp_touch2, temp_touch3, temp_touch4;
    imu_num << imu->getRollPitchYaw()[0] + 0.5 * PI, - imu->getRollPitchYaw()[1], imu->getRollPitchYaw()[2] - 0.5 * PI;
    cout << "imu:   "<< imu_num.transpose()<<endl;
    global_v = gps->getSpeed();


    if (imu_num(2) <= -PI)
        imu_num(2) = imu_num(2) + 2 * PI;
    else if (imu_num(2) > -PI && imu_num(2) <= -0.5)
        imu_num(2) = imu_num(2) + 2 * PI;

    for (int i = 0; i < 12; i++)
    {
        jointPresentVel(i) = (jointPresentPos(i) - jointLastPos(i))/ timePeriod;
    }

    for (int i = 0; i < 3; i++)
    {
        imuVel(i) = (imu_num(i) - Lastimu_num(i)) / timePeriod;
    }
    for (int i = 0; i < 6; i++)
    {
        distance(i) = Ds[i]->getValue();
    }
    if (key == 87)
    {
        tCV(0) = tCV(0) + 0.001;
        // tCV[0] = 0.12;
    }
    else if (key == 83)
    {
      tCV(0) = tCV(0) - 0.001;
    }
    else if (key == 65)
    {
      tCV(1) = tCV(1) + 0.001;
    }
    else if (key == 68)
    {
      tCV(1) = tCV(1) - 0.001;
    }
    else if(key == 81)
    {
      tCV(2) = tCV(2) + 0.001;
    }
    else if(key == 69)
    {
      tCV(2) = tCV(2) - 0.001;
    }
    else if(key == 90)
    {
        change_judge = 1;
    }
    else
    {
      tCV = tCV;
    }
    // if (distance[2]> 12.48 && distance[2]<= 13)
    //     tCV(0) = 0.13;
    // else if (distance[2]> 13 && distance[2] <=14.1)
    //     tCV(0) = 0;
    // else if (distance[2] > 14.1)
    //     tCV(0) = 0.13;
    targetCoMVelocity = tCV;
}
void MotionControl::forwardKinematics()
{
    float joint_pres_pos[4][3];
    joint_pres_pos[0][0] = jointPresentPos[0];
    joint_pres_pos[0][1] = jointPresentPos[1];
    joint_pres_pos[0][2] = jointPresentPos[2];
    joint_pres_pos[1][0] = jointPresentPos[3];
    joint_pres_pos[1][1] = jointPresentPos[4];
    joint_pres_pos[1][2] = jointPresentPos[5];
    joint_pres_pos[2][0] = jointPresentPos[6];
    joint_pres_pos[2][1] = jointPresentPos[7];
    joint_pres_pos[2][2] = jointPresentPos[8];
    joint_pres_pos[3][0] = jointPresentPos[9];
    joint_pres_pos[3][1] = jointPresentPos[10];
    joint_pres_pos[3][2] = jointPresentPos[11];
    for(int leg_nums = 0; leg_nums < 4; leg_nums++)
    {
        legPresentPos(leg_nums,0) = -L1 * sin(joint_pres_pos[leg_nums][1]) - L2 * sin(joint_pres_pos[leg_nums][1] + joint_pres_pos[leg_nums][2]);
        legPresentPos(leg_nums,1) = L1 * sin(joint_pres_pos[leg_nums][0]) * cos(joint_pres_pos[leg_nums][1]) + L2 * sin(joint_pres_pos[leg_nums][0]) * cos(joint_pres_pos[leg_nums][1] + joint_pres_pos[leg_nums][2]);
        legPresentPos(leg_nums,2) = -L1 * cos(joint_pres_pos[leg_nums][0]) * cos(joint_pres_pos[leg_nums][1]) - L2 * cos(joint_pres_pos[leg_nums][0]) * cos(joint_pres_pos[leg_nums][1] + joint_pres_pos[leg_nums][2]);
        leg2CoMPrePos(leg_nums,0) = shoulderPos(leg_nums,0) + legPresentPos(leg_nums,0);
        leg2CoMPrePos(leg_nums,1) = shoulderPos(leg_nums,1) + legPresentPos(leg_nums,1);
        leg2CoMPrePos(leg_nums,2) = legPresentPos(leg_nums,2);
    }
}
void MotionControl::jacobians()
{
    jacobian(0 ,0) = 0;
    jacobian(0 ,1) =  -L1 * cos(jointPresentPos(1)) -L2 * cos(jointPresentPos(1) + jointPresentPos(2));
    jacobian(0 ,2) =  -L2 * cos(jointPresentPos(1) + jointPresentPos(2));
    jacobian(0 ,3) = L1 * cos(jointPresentPos(0)) * cos(jointPresentPos(1)) + L2 * cos(jointPresentPos(0)) * cos(jointPresentPos(1) + jointPresentPos(2));
    jacobian(0 ,4) = -L1 * sin(jointPresentPos(0)) * sin(jointPresentPos(1)) - L2 * sin(jointPresentPos(0)) * sin(jointPresentPos(1) + jointPresentPos(2));
    jacobian(0 ,5) = -L2 * sin(jointPresentPos(0)) * sin(jointPresentPos(1) + jointPresentPos(2));
    jacobian(0 ,6) = L1 * sin(jointPresentPos(0)) * cos(jointPresentPos(1)) + L2 * sin(jointPresentPos(0)) * cos(jointPresentPos(1) + jointPresentPos(2));
    jacobian(0 ,7) = L1 * cos(jointPresentPos(0)) * sin(jointPresentPos(1)) + L2 * cos(jointPresentPos(0)) * sin(jointPresentPos(1) + jointPresentPos(2));
    jacobian(0 ,8) = L2 * cos(jointPresentPos(0)) * sin(jointPresentPos(1) + jointPresentPos(2));

    jacobian(1 ,0) = 0;
    jacobian(1 ,1) =  -L1 * cos(jointPresentPos(4)) -L2 * cos(jointPresentPos(4) + jointPresentPos(5));
    jacobian(1 ,2) =  -L2 * cos(jointPresentPos(4) + jointPresentPos(5));
    jacobian(1 ,3) = L1 * cos(jointPresentPos(3)) * cos(jointPresentPos(4)) + L2 * cos(jointPresentPos(3)) * cos(jointPresentPos(4) + jointPresentPos(5));
    jacobian(1 ,4) = -L1 * sin(jointPresentPos(3)) * sin(jointPresentPos(4)) - L2 * sin(jointPresentPos(3)) * sin(jointPresentPos(4) + jointPresentPos(5));
    jacobian(1 ,5) = -L2 * sin(jointPresentPos(3)) * sin(jointPresentPos(4) + jointPresentPos(5));
    jacobian(1 ,6) = L1 * sin(jointPresentPos(3)) * cos(jointPresentPos(4)) + L2 * sin(jointPresentPos(3)) * cos(jointPresentPos(4) + jointPresentPos(5));
    jacobian(1 ,7) = L1 * cos(jointPresentPos(3)) * sin(jointPresentPos(4)) + L2 * cos(jointPresentPos(3)) * sin(jointPresentPos(4) + jointPresentPos(5));
    jacobian(1 ,8) = L2 * cos(jointPresentPos(3)) * sin(jointPresentPos(4) + jointPresentPos(5));

    jacobian(2, 0) = 0;
    jacobian(2 ,1) =  -L1 * cos(jointPresentPos(7)) -L2 * cos(jointPresentPos(7) + jointPresentPos(8));
    jacobian(2 ,2) =  -L2 * cos(jointPresentPos(7) + jointPresentPos(8));
    jacobian(2 ,3) = L1 * cos(jointPresentPos(6)) * cos(jointPresentPos(7)) + L2 * cos(jointPresentPos(6)) * cos(jointPresentPos(7) + jointPresentPos(8));
    jacobian(2 ,4) = -L1 * sin(jointPresentPos(6)) * sin(jointPresentPos(7)) - L2 * sin(jointPresentPos(6)) * sin(jointPresentPos(7) + jointPresentPos(8));
    jacobian(2 ,5) = -L2 * sin(jointPresentPos(6)) * sin(jointPresentPos(7) + jointPresentPos(8));
    jacobian(2 ,6) = L1 * sin(jointPresentPos(6)) * cos(jointPresentPos(7)) + L2 * sin(jointPresentPos(6)) * cos(jointPresentPos(7) + jointPresentPos(8));
    jacobian(2 ,7) = L1 * cos(jointPresentPos(6)) * sin(jointPresentPos(7)) + L2 * cos(jointPresentPos(6)) * sin(jointPresentPos(7) + jointPresentPos(8));
    jacobian(2 ,8) = L2 * cos(jointPresentPos(6)) * sin(jointPresentPos(7) + jointPresentPos(8));

    jacobian(3 ,0) = 0;
    jacobian(3 ,1) =  -L1 * cos(jointPresentPos(10)) -L2 * cos(jointPresentPos(10) + jointPresentPos(11));
    jacobian(3 ,2) =  -L2 * cos(jointPresentPos(10) + jointPresentPos(11));
    jacobian(3 ,3) = L1 * cos(jointPresentPos(9)) * cos(jointPresentPos(10)) + L2 * cos(jointPresentPos(9)) * cos(jointPresentPos(10) + jointPresentPos(11));
    jacobian(3 ,4) = -L1 * sin(jointPresentPos(9)) * sin(jointPresentPos(10)) - L2 * sin(jointPresentPos(9)) * sin(jointPresentPos(10) + jointPresentPos(11));
    jacobian(3 ,5) = -L2 * sin(jointPresentPos(9)) * sin(jointPresentPos(10) + jointPresentPos(11));
    jacobian(3 ,6) = L1 * sin(jointPresentPos(9)) * cos(jointPresentPos(10)) + L2 * sin(jointPresentPos(9)) * cos(jointPresentPos(10) + jointPresentPos(11));
    jacobian(3 ,7) = L1 * cos(jointPresentPos(9)) * sin(jointPresentPos(10)) + L2 * cos(jointPresentPos(9)) * sin(jointPresentPos(10) + jointPresentPos(11));
    jacobian(3 ,8) = L2 * cos(jointPresentPos(9)) * sin(jointPresentPos(10) + jointPresentPos(11));
    for (int i = 0; i < 4; i++)
    {
        for (int j = 0; j < 9; j++)
        {
            if (jacobian(i, j) < -10 || jacobian (i, j) > 10)
            {
                jacobian(i ,j) = 0.0;
                cout << "jacobian"<<endl;
            }
        }
    }
}
void MotionControl::state_judgement()
{
    pre_body_high = body_high;
    if (stateflag == 0)
        body_high = (legPresentPos(0, 2) + legPresentPos(3, 2))/2;
    else
        body_high = (legPresentPos(1, 2) + legPresentPos(2, 2))/2;
    body_highv = (body_high - pre_body_high)/timePeriod;
}
void MotionControl::inverseKinematics()
{
    float jo_ang[4][3] = {0};
    static int times = 0;
    if(times!=0)
    {
        for(int joints=0; joints<12; joints++)
        {
            jointCmdPosLast[joints] = jointPresentPos[joints];
            jointCmdPos[joints] = jointPresentPos[joints];
        }
    }
    for(int leg_num = 0; leg_num < 4; leg_num++)
    {
        float x = legCmdPos_input(leg_num,0);
        float y = legCmdPos_input(leg_num,1);
        float z = legCmdPos_input(leg_num,2);
        float theta0 = atan (- y / z);
        float theta2 = - acos ((pow(( sin(theta0) * y - cos(theta0) * z), 2)  + pow(x, 2) - pow(L1, 2) - pow(L2, 2)) / ( 2 * L1 * L2)) ; 
        float cos_theta1 = ((sin (theta2) * y - cos (theta0) * z) * (L1 + L2 * cos (theta2)) - x * L2 * sin(theta2)) / (pow((sin (theta2) * y - cos (theta0) * z), 2) + pow(x, 2));
        float sin_theta1 = ((L1 + L2 * cos (theta2)) * x + (sin (theta2) * y - cos (theta0) * z) * L2 * sin(theta2)) / (-pow((sin (theta2) * y - cos (theta0) * z), 2) - pow(x, 2));
        float theta1 = atan (sin_theta1/ cos_theta1);
        jo_ang[leg_num][0] = theta0;
        jo_ang[leg_num][1] = theta1;
        jo_ang[leg_num][2] = theta2;
    }
    jointCmdPos[0] = jo_ang[0][0];
    jointCmdPos[1] = jo_ang[0][1];
    jointCmdPos[2] = jo_ang[0][2];
    jointCmdPos[3] = jo_ang[1][0];
    jointCmdPos[4] = jo_ang[1][1];
    jointCmdPos[5] = jo_ang[1][2];
    jointCmdPos[6] = jo_ang[2][0];
    jointCmdPos[7] = jo_ang[2][1];
    jointCmdPos[8] = jo_ang[2][2];
    jointCmdPos[9] = jo_ang[3][0];
    jointCmdPos[10] = jo_ang[3][1];
    jointCmdPos[11] = jo_ang[3][2];
    if(times!=0)
    {
        for(int joints=0; joints<12; joints++)
        {
            jointCmdVel[joints] = (jointCmdPos[joints] - jointCmdPosLast[joints]) / timePeriod;
        }
    }
    else
    {
        for(int joints=0; joints<12; joints++)
        {
            jointCmdVel[joints] = 0;
        }
    }
    times++;
}
void MotionControl::Setjoint()
{
    for (int i= 0 ; i<12; i++)
    {
        if (jacobian_torque(i) < -2000 || jacobian_torque(i) > 2000)
        jacobian_torque(i) = 0.0;
    }
    Vector<float, 12> temp_jointCmdPos, temp_jointCmdVel;
    for(uint8_t joints=0; joints<12; joints++)
    {
        temp_jointCmdPos(joints) = jointCmdPos[joints];
        temp_jointCmdVel(joints) = jointCmdVel[joints];
    }
    Vector<float, 12> temp_motorCmdTorque;
    temp_motorCmdTorque = 0.0* (temp_jointCmdPos - jointPresentPos) + 0.0* (temp_jointCmdVel - jointPresentVel);
    jacobian_torque += temp_motorCmdTorque;
    for (int i = 0; i < 12; i++)
    {
        // Tor[i]->setTorque(jacobian_torque(i));
        Tor[i]->setPosition(jointCmdPos[i]);
        // Tor[i]->setVelocity(jointCmdPos[i]);
    }
}
void MotionControl::setCoMVel(Vector<float, 3> tCV)
{
    targetCoMVelocity = tCV;
}
void MotionControl::State_start()
{
    inverseKinematics();
    for (int i= 0; i<12; i++)
    {
        Tor[i]->setPosition(jointCmdPos[i]);
    }
    Start_time += timePeriod;
}
void MotionControl::stance_2leg()
{
    for (int i = 0; i<4; i++)
    {
        int legNum = i;
        if (state(i) == 1 && statePoint(i) > 0.05)
            stateFlag(i) = 1;
        if (stateFlag(i) == 1)
        {
            if(statePoint(i) < 1e-4)  // if on the start pos
            {
                stancePhaseStartPos.row(legNum) = legCmdPos.row(legNum);
                for(uint8_t pos=0; pos<3; pos++)
                targetCoMPosition(legNum, pos) = 0.0;
            }
            Matrix<float, 3, 3> trans;
            trans<<cos(targetCoMPosition(legNum,2)), -sin(targetCoMPosition(legNum,2)), targetCoMPosition(legNum,0),
                   sin(targetCoMPosition(legNum,2)), cos(targetCoMPosition(legNum,2)), targetCoMPosition(legNum,1),
                   0, 0, 1;
            Matrix<float, 3, 1> oneShoulderPos_3x1;
            oneShoulderPos_3x1<<shoulderPos.row(legNum)(0), shoulderPos.row(legNum)(1), 1;
            oneShoulderPos_3x1 = trans * oneShoulderPos_3x1;

            if(statePoint(i) < 1e-4)  // if on the start pos
            {
                stancePhaseStartPos.row(legNum) = legCmdPos.row(legNum);
                shoulderPos(legNum, 0) = oneShoulderPos_3x1(0);
                shoulderPos(legNum, 1) = oneShoulderPos_3x1(1);
            }
            statePoint(i) += 0.01;
            if(state.sum() == 4 || state.sum() == 0)  // if on the end pos
            {
                stancePhaseEndPos.row(legNum) = legCmdPos.row(legNum);
                statePoint(i) = 0.0;
            }
            legCmdPos(legNum, 0) = stancePhaseStartPos(legNum, 0) + (shoulderPos(legNum, 0) - oneShoulderPos_3x1(0));
            legCmdPos(legNum, 1) = stancePhaseStartPos(legNum, 1) + (shoulderPos(legNum, 1) - oneShoulderPos_3x1(1));
        }
        else
        {
            Matrix<float, 1, 3> swingPhaseVelocity = (stancePhaseEndPos.row(legNum) - stancePhaseStartPos.row(legNum)) / 0.29;
            for(uint8_t pos=0; pos<3; pos++)
            legCmdPos(legNum, pos) = legCmdPos(legNum, pos) - swingPhaseVelocity(pos) * timePeriod;

            if(timePresent >= 0.15)
            {
                legCmdPos(legNum, 2) -= 0.01;
                // float temp_a =  timePresentForSwing(legNum) - (timeForGaitPeriod - (timeForStancePhase(legNum,1) - timeForStancePhase(legNum,0)))/2;
                // legCmdPos(legNum, 2) = -0.24 + 0.05 * sin(temp_a * PI/(timeForGaitPeriod - (timeForStancePhase(legNum,1) - timeForStancePhase(legNum,0))) + 0.5 * PI);
            }
            if(timePresent < 0.15)
            {
                legCmdPos(legNum, 2) += 0.01 ;
                // float temp_a =  timePresentForSwing(legNum);
                // legCmdPos(legNum, 2) = -0.24 + 0.05 * sin(temp_a * PI/(timeForGaitPeriod - (timeForStancePhase(legNum,1) - timeForStancePhase(legNum,0))));
            }
        }
    }
    timePresent += timePeriod;
    for(uint8_t leg=0; leg<4; leg++)
    {
        for(uint8_t pos=0; pos<3; pos++)
        {
            targetCoMPosition(leg, pos) += targetCoMVelocity(pos) * timePeriod;
        }
    }

    if (state.sum() == 4 || state.sum() == 0)  // check if present time has reach the gait period
    {                                                            // if so, set it to 0.0
        timePresent = 0.0;
        if (stateFlag(0) + stateFlag(3) == 2)
        stateFlag << 0,1,1,0;
        else
        stateFlag << 1,0,0,1;
    }

}
void MotionControl::swing_2leg()
{
    //摆动相轨迹设计
}
void MotionControl::nextStep()
{
    float timePresent = fly_time;
    float H = 0.05;
    float t = fly_time;
    float sgn;
    if ( t >= 0 && t < T/2)
    {
       sgn = 1;
    }
    else
    {
       sgn = -1;
    }
    float z ;
    z = H * (sgn * (2*(t/T-sin(4*PI*t/T)/(4*PI))-1)+1);

    for (int legNum = 0; legNum < 4; legNum++)
    {
        if (timePresent <= 0.005)
        {
            legCmdPos(legNum, 0) = legPresentPos(legNum, 0);
            temp_x(legNum) = body_x  - legPresentPos(legNum, 0);
            legCmdPos(legNum, 1) = body_y;
            legCmdPos_pre(legNum, 2) = legPresentPos(legNum, 2);
        }

        for(uint8_t pos=0; pos<3; pos++)
        {
            if (first_state == 0)
            targetCoMPosition(legNum, pos) = 0.5 * targetCoMVelocity(pos) * timePeriod;
            else
            targetCoMPosition(legNum, pos) = targetCoMVelocity(pos) * timePeriod;
        }
        if (stateFlag(legNum) == 0)
        {
            Matrix<float, 3, 3> trans;
            trans<<cos(targetCoMPosition(legNum,2)), -sin(targetCoMPosition(legNum,2)), targetCoMPosition(legNum,0),
                    sin(targetCoMPosition(legNum,2)), cos(targetCoMPosition(legNum,2)), targetCoMPosition(legNum,1),
                    0, 0, 1;
            Matrix<float, 3, 1> oneShoulderPos_3;
            Matrix<float, 3, 1> oneShoulderPos_3x1;
            oneShoulderPos_3x1<<shoulderPos.row(legNum)(0), shoulderPos.row(legNum)(1), 1;
            oneShoulderPos_3 = trans * oneShoulderPos_3x1;
            legCmdPos(legNum, 0) += oneShoulderPos_3(0) - oneShoulderPos_3x1(0) + temp_x(legNum) * 0.01/T;
            legCmdPos(legNum, 1) += oneShoulderPos_3(1) - oneShoulderPos_3x1(1);
            legCmdPos(legNum, 2) = legCmdPos_pre(legNum, 2) + z; 
        }
        else
        {
            Matrix<float, 3, 3> trans;
            trans<<cos(targetCoMPosition(legNum,2)), -sin(targetCoMPosition(legNum,2)), targetCoMPosition(legNum,0),
                    sin(targetCoMPosition(legNum,2)), cos(targetCoMPosition(legNum,2)), targetCoMPosition(legNum,1),
                    0, 0, 1;
            Matrix<float, 3, 1> oneShoulderPos_3;
            Matrix<float, 3, 1> oneShoulderPos_3x1;
            oneShoulderPos_3x1<<shoulderPos.row(legNum)(0), shoulderPos.row(legNum)(1), 1;
            oneShoulderPos_3 = trans * oneShoulderPos_3x1;
            if ((stateflag == 0 && (legNum == 0 || legNum == 3)) || (stateflag == 1 && (legNum == 1 || legNum == 2)))
            legCmdPos(legNum, 0) -= (oneShoulderPos_3(0) - oneShoulderPos_3x1(0)) - temp_x(legNum) * 0.01/T;
            else
            legCmdPos(legNum, 0) -= (oneShoulderPos_3(0) - oneShoulderPos_3x1(0));
            
            legCmdPos(legNum, 1) -= (oneShoulderPos_3(1) - oneShoulderPos_3x1(1));
            // legCmdPos(legNum, 2) = legCmdPos_pre(legNum, 2) + 0.1 * (-0.23 - legCmdPos_pre(legNum, 2));
            legCmdPos(legNum, 2) = -bodyh - 0.8 * (legPresentPos(legNum, 2) + 0.23); 
        }
    }
    fly_time += timePeriod;
}

void MotionControl::statechange()
{
    if (stateflag == 0 && fly_time > 0.5 * T)
    {
        if (state(1) == 1)
        stateFlag(1) = 1;
        if (state(2) == 1)
        stateFlag(2) = 1;
    }
    else if (stateflag == 1 && fly_time > 0.5 * T)
    {
        if (state(0) == 1)
        stateFlag(0) = 1;
        if (state(3) == 1)
        stateFlag(3) = 1;
    }
    if (fly_time > T + 0 * timePeriod && stateFlag.sum() >= 4)
    {
        if (first_state == 0)
        first_state = 1;
        if (stateflag == 0)
        {
            stateFlag << 0, 1, 1, 0;
            stateflag = 1;
            fly_time = 0;
        }
        else if (stateflag == 1)
        {
            stateFlag << 1, 0, 0, 1;
            stateflag = 0;
            fly_time = 0;
            cout << "change" <<endl;
        }
    }
}

void MotionControl::del_imu()
{
    //通过支撑腿来补偿横滚角的变化,会产生内力，需要优化，先做动态稳定
    float z = 2;
    float m = 0.001;
    float k = 0.08;//0.52
    float p = 2 * z * sqrt(m * k);
    for (int legNum = 0; legNum < 4; legNum++)
    {
        if (stateFlag(legNum) == 1)
        {
            if (legNum == 0 || legNum == 2)
            legCmdPos(legNum, 2) += k * (imu_num(0) - 0) + p * (imuVel(0));
            else
            legCmdPos(legNum, 2) += -k * (imu_num(0) - 0) - p * (imuVel(0));
        }
    }
    float k2 = k;//0.52
    float p2 = 2 * z * sqrt(m * k2);;//0.015
    for (int legNum = 0; legNum < 4; legNum++)
    {
        if (stateFlag(legNum) == 1)
        {
            if (legNum == 0 || legNum == 1)
            legCmdPos(legNum, 2) += -k2 * (imu_num(1) - 0) - p2 * imuVel(1);
            else
            legCmdPos(legNum, 2) += k2 * (imu_num(1) - 0) + p2 * imuVel(1);
        }
    }

    //摆动腿由于质心状态的变化，会出现没有摆到的情况
    for (int legNum = 0; legNum < 4; legNum++)
    {
        if (stateFlag(legNum) == 0)
        {
            legCmdPos_input(legNum, 0) = legCmdPos(legNum, 0) +  0.5 * (legCmdPos(legNum, 0) - legPresentPos(legNum, 0));
            legCmdPos_input(legNum, 1) = legCmdPos(legNum, 1) +  0.5 * (legCmdPos(legNum, 1) - legPresentPos(legNum, 1));
            legCmdPos_input(legNum, 2) = legCmdPos(legNum, 2) +  2.2 * (legCmdPos(legNum, 2) - legPresentPos(legNum, 2));
        }
        else
        {
            legCmdPos_input(legNum, 0) = legCmdPos(legNum, 0);
            legCmdPos_input(legNum, 1) = legCmdPos(legNum, 1);
            legCmdPos_input(legNum, 2) = legCmdPos(legNum, 2);
        }
    
        // if(legCmdPos(legNum,2) < -0.24)
        // legCmdPos(legNum,2) = -0.24;
    }
}

void MotionControl::changestate()
{
    //change posture
    int fly_leg = surmountFlag;
    float T = change_T;
    int temp_judge = change_judge;
    static float temp_high[4],temp_x[4],temp_y[4];
    float S;
    if (surmountFlag == 0 )
    S = 0.06;//0.042
    else if (surmountFlag == 1)
    S = 0.04;//0.0
    else
    S = 0.03;
    float temp_H = 0.07;
    float t = fly_time;
    float y = targetCoMVelocity(1) * t;
    float x;
    float z;
    if (t <= 0.2 * T)
    {
        x = 0;
    }
    else if (t >=  0.2 * T && t < 0.8 * T)
    {
        x = (t - 0.2 * T) * S / (0.6 * T);
    }
    else
    {
        x = S;
    }
    float sgn;
    if ( t >= 0 && t < T/2)
    {
       sgn = 1;
    }
    else
    {
       sgn = -1;
    }
    z = temp_H * (sgn * (2*(t/T-sin(4*PI*t/T)/(4*PI))-1)+1);
    Vector<float, 4>detx;
    Vector<float, 4>dety;
    detx << 0.5 * S, 0.5 * S, 0.8 * S, 1 * S;
    dety << 0, 0, 0, y;
    for(int i = 0; i < 4; i++)
    {
        if (timePresent < 0.005)
        {
            temp_high[i] = 0;
            temp_x[i] = 0;
            temp_y[i] = 0;
        }
        if (i != fly_leg)
        {
            legCmdPos(i, 0) = legCmdPos(i, 0) - detx(fly_leg)/(change_T/timePeriod);
            legCmdPos(i, 1) = legCmdPos(i, 1) - dety(fly_leg)/(change_T/timePeriod);
            legCmdPos(i, 2) = legCmdPos(i, 2);
        }
        else
        {
            legCmdPos(fly_leg, 0) = temp_x[i] + x;
            legCmdPos(fly_leg, 1) = temp_y[i] + y;
            legCmdPos(fly_leg, 2) = -bodyh + z + temp_high[i];
        }
        // if (fly_leg < 2)
        // {
        //     if (i != fly_leg)
        //     {
        //         if (fly_leg < 2)
        //         {
        //             legCmdPos(i, 0) = legCmdPos(i, 0) - detx(fly_leg)/50;
        //             legCmdPos(i, 1) = legCmdPos(i, 1) - dety(fly_leg)/50;
        //             legCmdPos(i, 2) = legCmdPos(i, 2);
        //         }
        //         else
        //         {
        //             if (i < fly_leg)
        //             {
        //                 legCmdPos(i, 0) = legCmdPos(i, 0) - detx(fly_leg)/50;
        //                 legCmdPos(i, 1) = legCmdPos(i, 1) - dety(fly_leg)/50;
        //                 legCmdPos(i, 2) = legCmdPos(i, 2);
        //             }
        //             else
        //             {
        //                 legCmdPos(i, 1) = legCmdPos(i, 1) + dety(fly_leg)/50;
        //             }
        //         }
        //     }
        //     else
        //     {
        //         legCmdPos(fly_leg, 0) = temp_x[i] + x;
        //         legCmdPos(fly_leg, 1) = temp_y[i] + y;
        //         legCmdPos(fly_leg, 2) = -0.23 + z + temp_high[i];
        //     }
        // }
        // else if (fly_leg == 2)
        // {
        //     if (i == 0 || i == 3)
        //     {
        //         legCmdPos(i, 0) = legCmdPos(i, 0) - detx(fly_leg)/50;
        //         legCmdPos(i, 1) = legCmdPos(i, 1) - dety(fly_leg)/50;
        //         legCmdPos(i, 2) = legCmdPos(i, 2) ;
        //     }
        //     else if ( i == 1)
        //     {
        //         legCmdPos(fly_leg, 2) = -0.23 + z + temp_high[i];
        //     }
        //     else
        //     {
        //         legCmdPos(fly_leg, 0) = temp_x[i] + x;
        //         legCmdPos(fly_leg, 1) = temp_y[i] + y;
        //         legCmdPos(fly_leg, 2) = -0.23 + z + temp_high[i];
        //     }
        // }
        // else
        // {
        //     if (i == 1 || i == 2)
        //     {
        //         legCmdPos(i, 0) = legCmdPos(i, 0) - detx(fly_leg)/50;
        //         legCmdPos(i, 1) = legCmdPos(i, 1) - dety(fly_leg)/50;
        //         legCmdPos(i, 2) = legCmdPos(i, 2) ;
        //     }
        //     else if (i == 0)
        //     {
        //         legCmdPos(fly_leg, 2) = z + temp_high[i];
        //     }
        //     else
        //     {
        //         legCmdPos(fly_leg, 0) = temp_x[i] + x;
        //         legCmdPos(fly_leg, 1) = temp_y[i] + y;
        //         legCmdPos(fly_leg, 2) = -0.23 + z + temp_high[i];
        //     }//* code */
        // }    
    }
    for (int legNum = 0; legNum < 4; legNum++)
    {
        for(int i = 0; i < 3; i++)
        {
            legCmdPos_input(legNum, i) = legCmdPos(legNum, i);
        }
    }
        
    fly_time += timePeriod;
}

void MotionControl::stance_4leg()
{
    //4 legs stance
    float com_x;
    // if (surmountFlag == 0)
    // {
    //     com_x = -1;
    // }
    // else if (surmountFlag == 1)
    // {
    //     com_x = -1;
    // }
    // else if (surmountFlag == 2)
    // {
    //     com_x = -1;
    // }
    // else
    // {
    //     com_x = -1;
    // }
    com_x = 0/100;
    // float com_x = targetCoMVelocity(0) * timePeriod;
    // float com_y = targetCoMVelocity(1) * timePeriod;
    // float com_z = (legCmdPos(0, 2) + legCmdPos(1, 2) + legCmdPos(2, 2) + legCmdPos(3, 2))/4;
    for (int i = 0; i < 4; i++)
    {
        legCmdPos(i, 0) = legCmdPos(i, 0) + com_x/40;
        legCmdPos(i, 1) = legCmdPos(i, 1);
        legCmdPos(i, 2) = legCmdPos(i, 2);
    }
    for (int legNum = 0; legNum < 4; legNum++)
    {
        for(int i = 0; i < 3; i++)
        {
            legCmdPos_input(legNum, i) = legCmdPos(legNum, i);
        }
    }
}